package scu.maqiang.numeric;

import java.util.Arrays;
import java.util.function.Function;

import scu.maqiang.mesh.MatrixFunc;
import scu.maqiang.mesh.ScalarFunc;

/**
 * 最小二乘法拟合求解器
 * @author 马强
 *
 */
public class MLSSolver {

	public static void ExpModel(double[] t, double[] y, double[] param, double[] error) {
		int n = t.length;
		double[][] AT = new double[2][];
		AT[0] = new double[n];
		Arrays.fill(AT[0], 1.0);
		AT[1] = t;
		double[] b = new double[n];
		Arrays.setAll(b, i -> Math.log(y[i]));
		double[][] ATA = MVO.matmul(AT, MVO.transpose(AT));
		double[] ATb = MVO.matmul(AT, b);
		double detATA = MVO.determinant(ATA);
		double k = (ATb[0] * ATA[1][1] - ATb[1] * ATA[0][1]) / detATA;
		System.out.println(k);
		param[0] = Math.exp(k);
		param[1] = (ATA[0][0] * ATb[1] - ATA[1][0] * ATb[0]) / detATA;
		double temp = 0.0;
		double temp1 = 0.0;
		for(int i = 0; i < n; i++) {
			double temp0 = y[i] - param[0] * Math.exp(param[1] * t[i]);
			temp += temp0 * temp0;
			temp0 = b[i] - k - param[1] * t[i];
			temp1 += temp0 * temp0;
		}
		error[0] = Math.sqrt(temp / n);
		error[1] = Math.sqrt(temp);
		error[2] = Math.sqrt(temp1 / n);
	}
	
	public static void PowerModel(double[] t, double[] y, double[] param, double[] error) {
		int n = t.length;
		double[][] AT = new double[2][];
		AT[0] = new double[n];
		Arrays.fill(AT[0], 1.0);
		Arrays.setAll(AT[1], i -> Math.log(t[i]));
		double[] b = new double[n];
		Arrays.setAll(b, i -> Math.log(y[i]));
		double[][] ATA = MVO.matmul(AT, MVO.transpose(AT));
		double[] ATb = MVO.matmul(AT, b);
		double detATA = MVO.determinant(ATA);
		double k = (ATb[0] * ATA[1][1] - ATb[1] * ATA[0][1]) / detATA;
		System.out.println(k);
		param[0] = Math.exp(k);
		param[1] = (ATA[0][0] * ATb[1] - ATA[1][0] * ATb[0]) / detATA;
		double temp = 0.0;
		double temp1 = 0.0;
		for(int i = 0; i < n; i++) {
			double temp0 = y[i] - param[0] * Math.pow(t[i], param[1]);
			temp += temp0 * temp0;
			temp0 = b[i] - k - param[1] * t[i];
			temp1 += temp0 * temp0;
		}
		error[0] = Math.sqrt(temp / n);
		error[1] = Math.sqrt(temp);
		error[2] = Math.sqrt(temp1 / n);
	}
	
	public static void DrugConcentrationModel(double[] t, double[] y, double[] param, double[] error) {
		int n = t.length;
		double[][] AT = new double[2][];
		AT[0] = new double[n];
		Arrays.fill(AT[0], 1.0);
		AT[1] = t;
		double[] b = new double[n];
		Arrays.setAll(b, i -> Math.log(y[i]) - Math.log(t[i]));
		double[][] ATA = MVO.matmul(AT, MVO.transpose(AT));
		double[] ATb = MVO.matmul(AT, b);
		double detATA = MVO.determinant(ATA);
		double k = (ATb[0] * ATA[1][1] - ATb[1] * ATA[0][1]) / detATA;
		System.out.println(k);
		param[0] = Math.exp(k);
		param[1] = (ATA[0][0] * ATb[1] - ATA[1][0] * ATb[0]) / detATA;
		double temp = 0.0;
		double temp1 = 0.0;
		for(int i = 0; i < n; i++) {
			double temp0 = y[i] - param[0] * t[i] * Math.exp(param[1] * t[i]);
			temp += temp0 * temp0;
			temp0 = b[i] - k - param[1] * t[i];
			temp1 += temp0 * temp0;
		}
		error[0] = Math.sqrt(temp / n);
		error[1] = Math.sqrt(temp);
		error[2] = Math.sqrt(temp1 / n);
	}
	
	public static double[] polyfit(double[] x, double[] y, int degree, double[] error) {
		int n = x.length;
		int m = degree + 1;
		double[] coef = new double[m];
		double[][] AT = new double[m][];
		AT[0] = new double[n];
		Arrays.fill(AT[0], 1.0);
		for(int i = 1; i < m; i++) {
			AT[i] = new double[n];
			for(int j = 0; j < n; j++) {
				AT[i][j] = Math.pow(x[j], i);
			}
		}
		
		double[][] ATA = MVO.matmul(AT, MVO.transpose(AT));
		double[] ATb = MVO.matmul(AT, y);
		
		DirectDSolver dds = new DirectDSolver(ATA);
		dds.CholeskyFactorize2();
		dds.CholeskySolve2(ATb, coef);
		
		double temp = 0.0, temp0;
		double[] bb = new double[degree];
		for(int i = 0; i < n; i++) {
			temp0 = y[i] - MVO.polyval(coef, bb, x[i]);
			temp += temp0 * temp0;
		}
		error[0] = Math.sqrt(temp);
		error[1] = Math.sqrt(temp / n);
		return coef;
	}

	public static double[] ployval(double[] coef, double[] t) {
		int n = coef.length - 1;
		double result[] = new double[t.length];
		for(int k = 0; k < t.length; k++) {
			result[k] = coef[n];
			for (int i = n - 1; i >= 0; i--) {
				result[k] = result[k] * t[k] + coef[i];
			}
		}
		return result;
	}
	
	public static double[] LevenbergMarquardt(ScalarFunc[] r, double[] param0, double[] x, double[] y, MatrixFunc A, double[][][] paramA, double lambda, double[] error, double tol, int maxIteration) {
		double[] param1 = param0.clone();
		int nParam = param0.length;
		int nR = r.length;
		double[] RHS = new double[nR];
		double[] xx = new double[nR];
		for(int i = 0; i < nR; i++) {
			xx[0] = x[i];
			RHS[i] = y[i] - r[i].action(xx, 0, param0);
		}
		double[][] x00 = new double[1][];
		x00[0] = x;
		double[][] D = A.action(x, 0, paramA);
		//System.out.println(MVO.toString(D));
		//System.exit(0);
		double[][] DT = MVO.transpose(D);
		double[][] DTD = MVO.matmul(DT, D);
		for(int i = 0; i < nParam; i++) {
			DTD[i][i] *= (1 + lambda);
		}
		double[] DTRHS = MVO.matmul(DT, RHS);
		DirectDSolver dsolver = new DirectDSolver(DTD);
		dsolver.PGaussSolve(DTRHS, param1);
		for(int i = 0; i < nParam; i++) {
			param1[i] += param0[i];
		}
		System.out.println(0 + "\t" + Arrays.toString(param0));
		int i = 1;
		while(MVO.L2Norm(MVO.add(param1, -1.0, param0)) > tol) {
			System.out.println(i + "\t" + Arrays.toString(param1));
			Arrays.setAll(param0, idx -> param1[idx]);
			for(int j = 0; j < nR; j++) {
				xx[0] = x[j];
				RHS[j] = y[j] - r[j].action(xx, 0, param0);
			}
			D = A.action(x, 0, paramA);
			MVO.transpose(D, DT);
			MVO.matmul(DT, D, DTD);
			for(int k = 0; k < nParam; k++) {
				DTD[k][k] *= (1 + lambda);
			}
			MVO.matmul(DT, RHS, DTRHS);
			
			dsolver.setMatrix(DTD);
			dsolver.PGaussSolve(DTRHS, param1);
			for(int j = 0; j < nParam; j++) {
				param1[j] += param0[j];
			}
			i++;
			if (i >= maxIteration) {
				System.out.println("Iteration Has Reached Maximum Time!!");
				break;
			}
		}
		double temp = 0.0;
		for(i = 0; i < nR; i++) {
			xx[0] = x[i];
			double temp0 = y[i] -r[i].action(xx, 0, param1);
			temp += temp0 * temp0;
		}
		error[0] = Math.sqrt(temp / nR);
		error[1] = Math.sqrt(temp);
		return param1;
	}
	
	
	
	public static void main(String[] args) {
		
		//double[] t = {1950, 1955, 1960, 1965, 1970, 1975, 1980};
		/*
		double[] t = {0, 5, 10, 15, 20, 25, 30};
		double[] y = {53.05, 73.04, 98.31, 139.78, 193.48, 260.20, 320.39};
		double[] param = new double[2];
		double[] error = new double[3];
		ExpModel(t, y, param, error);
		System.out.println(Arrays.toString(param));
		System.out.println(Arrays.toString(error));
		*/
		
		/*
		double[] x = {-1.0, 0.0, 1.0, 2.0};
		double[] y = {1.0, 0.0, 0.0, -2.0};
		double[] error = new double[2];
		double[] coef = polyfit(x, y, 1, error);
		System.out.println(Arrays.toString(coef));
		double[] b = new double[3];
		for(int i = 0; i < 4; i++) {
			System.out.println(MVO.polyval(coef, b, x[i]));
		}
		System.out.println(Arrays.toString(error));
		*/

/*		
		double[] t = {1, 2, 3, 4, 5, 6, 7, 8};
		double[] y = {8.0, 12.3, 15.5, 16.8, 17.1, 15.8, 15.2, 14.0};
		double[] error = new double[3];
		double[] param = new double[2];
		DrugConcentrationModel(t, y, param, error);
		System.out.println(Arrays.toString(param));
		System.out.println(Arrays.toString(error));
		
		double maxConcentrationTime = -1.0 / param[1];
		
		double maxConcentration = param[0] * maxConcentrationTime * Math.exp(param[1] * maxConcentrationTime);
		System.out.println("Max concentration: " + maxConcentration);
		Function<Double, Double> func = x -> param[0] * x * Math.exp(param[1] * x) - 0.5 * maxConcentration;
		Function<Double, Double> dfunc = x -> param[0] * Math.exp(param[1] * x) + param[0] * param[1] * x * Math.exp(param[1] * x);
		double x = NLESolver.Newton(func, dfunc, 8.0, 1.0e-10, 100);
		System.out.println("half life: " + (x - maxConcentrationTime));
*/
		
/*		
		double[] t = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
		double[] y = {6.2, 9.5, 12.3, 13.9, 14.6, 13.5, 13.3, 12.7, 12.4, 11.9};
		double[] error = new double[3];
		double[] param = new double[2];
		DrugConcentrationModel(t, y, param, error);
		System.out.println(Arrays.toString(param));
		System.out.println(Arrays.toString(error));
		
		double maxConcentrationTime = -1.0 / param[1];
		System.out.println("Max concentration time: " + maxConcentrationTime);
		double maxConcentration = param[0] * maxConcentrationTime * Math.exp(param[1] * maxConcentrationTime);
		System.out.println("Max concentration: " + maxConcentration);
		Function<Double, Double> func = x -> param[0] * x * Math.exp(param[1] * x) - 0.5 * maxConcentration;
		Function<Double, Double> dfunc = x -> param[0] * Math.exp(param[1] * x) + param[0] * param[1] * x * Math.exp(param[1] * x);
		double x = NLESolver.Newton(func, dfunc, 8.0, 1.0e-10, 100);
		System.out.println("half life: " + (x - maxConcentrationTime));
		System.out.println();
		
		func = xx -> param[0] * xx * Math.exp(param[1] * xx) - 4.0;
		double x0 = NLESolver.Newton(func, dfunc, 8.0, 1.0e-10, 100);
		double x1 = NLESolver.Newton(func, dfunc, 0.0, 1.0e-10, 100);
		System.out.println("Effective time: " + (x0 - x1));
*/
		
//		double[] t = {1, 2, 2, 3, 4};
//		double[] y = {3, 5, 7, 5, 1};
//		double[] param = {1, 1, 1};
//		ScalarFunc[] func = new ScalarFunc[5];
//		func[0] = (xy, label, para) -> para[0] * Math.exp(-para[1] * ( xy[0] - para[2]) * (xy[0] - para[2]));
//		func[1] = func[2] = func[3] = func[4] = func[0];
//		double[] error = new double[2];
//		MatrixFunc A = (xy, label, para) -> {
//			double[][] mat = new double[5][3];
//			double t1, t2;
//			for(int i = 0; i < 5; i++) {
//				t2 = (xy[i] - para[2]) * (xy[i] - para[2]);
//				t1 = Math.exp(-para[1] * t2);
//				mat[i][0] = t1;
//				mat[i][1] = -para[0] * t2 * t1;
//				mat[i][2] = 2 * para[0] * para[1] * (xy[i] - para[2]) * t1;
//			}
//			return mat;
//		};
//		double[] coef = LevenbergMarquardt(func, param, t, y, A, 50, error, 1.0e-6, 5000);
//		System.out.println(Arrays.toString(coef));

		double[] T = {20.5, 26, 32.7, 40, 51, 61, 73, 80, 88, 95.7};
		double[] R = {765, 790, 826, 850, 873, 910, 942, 980, 1010, 1032};
		double[] error = new double[2];
		double[] coef = polyfit(T, R, 1, error);
		double[] z = ployval(coef, T);
		System.out.println(Arrays.toString(error));
		System.out.println(Arrays.toString(coef));
		System.out.println(Arrays.toString(z));
	}

}
